/*
A program, soon to be library, to make the bot travel a set distance and 
turn a set number of degrees. It works! Need to encorporate into a library.
*/



#define SPEED 1000                //Motor speed
#define TURN_SPEED 600            //Motor speed when making a turn
#define MR 1                     //Right Motor pin#
#define ML 0                     //Left Motor pin#
#define pi 3.141592
#define WHEEL_DIAMETER 5.5
#define TICKS_PER_ROTATION 1100.0
#define WHEEL_BASE 12.9

float WHEEL_CIRCUMFRENCE = pi*WHEEL_DIAMETER;


void travel(float distance){
    
    float ticks = distance*TICKS_PER_ROTATION/WHEEL_CIRCUMFRENCE;
    printf("Traveling %f cm.\n", distance);
    mrp(MR, SPEED,(long)ticks);
    mrp(ML, SPEED,(long)ticks);
    bmd(MR);
    bmd(ML);
}

void turn(float degrees){
    //Ticks = theta*(1100R/360r)
    float ticks = degrees*TICKS_PER_ROTATION*(WHEEL_BASE/2.0)/(360.0*WHEEL_DIAMETER/2.0);
    printf("Traveling %f degrees.\n", degrees);
    mrp(ML, TURN_SPEED, -(long)ticks);
    mrp(MR, TURN_SPEED, (long)ticks);
    bmd(MR);
    bmd(ML);
}

void stop(){
    
    printf("Stopping.\n");
    freeze(MR);
    freeze(ML);
}

